using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using surfInterop;
using DotNetMatrix;
using System.Drawing.Drawing2D;


namespace Blimpbots.Recognition
{
    public class Visualization
    {
        public static RectangleF PointRectangleF(Point2D pt, float diam)
        {
            PointF ptF = PointF.Subtract(pt.ToPointF(), new SizeF(diam / 2, diam / 2));
            return new RectangleF(ptF, new SizeF(diam, diam));
        }

        private static void PrepareGraphics(Graphics g)
        {
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
        }

        internal static void DrawPoints(IEnumerable<Point2D> pts, Graphics g)
        {            
            foreach(Point2D p in pts)
                g.DrawEllipse(Pens.Pink, (float)p.x - 1F, (float)p.y - 1F, 2F, 2F);
        }

        internal static void DrawCentroids(IEnumerable<CentroidEstimate> estimates, Graphics g, Brush br)
        {
            PrepareGraphics(g);

            foreach (CentroidEstimate e in estimates)
            {
                float diam = 10.0F; //(float)e.Scale * 10.0F;
                g.FillEllipse(br,
                    (float)e.X - diam / 2, (float)e.Y - diam / 2, diam, diam);
            }

        }

        private static GeneralMatrix MatrixFromPoint2D(Point2D pt)
        {
            GeneralMatrix m = new GeneralMatrix(3, 1);
            m.Array[0][0] = pt.x;
            m.Array[1][0] = pt.y;
            m.Array[2][0] = 1.0;
            return m;
        }

        private static void HomogeneousNormalize(GeneralMatrix m)
        {
            for (int x = 0; x < m.ColumnDimension; x++)
            {
                double nFactor = m.Array[m.RowDimension - 1][x];
                for (int y = 0; y < m.RowDimension - 1; y++)
                    m.Array[y][x] = m.Array[y][x] / nFactor;
            }
        }

        // TODO: use ProjectPoint internally instead of dupe code
        public static void Project(IEnumerable<SurfKeypoint> keypoints, GeneralMatrix H, Graphics g, Brush b)
        {
            float diam = 8.0F;

            Region r = new Region();

            foreach (SurfKeypoint k in keypoints)
            {
                GeneralMatrix k_Mat = MatrixFromPoint2D(new Point2D(k.x, k.y)); 
                GeneralMatrix projected = H * k_Mat;
                HomogeneousNormalize(projected);

                g.FillEllipse(b,
                    (float)projected.Array[0][0] - diam / 2, (float)projected.Array[1][0] - diam / 2, diam, diam);
            }
        }

        static Font tagFont = new Font("Trebuchet MS", 10.0f, FontStyle.Regular);
        static Brush tagBrush = new SolidBrush(Color.FromArgb(128, Color.White));

        public static void DrawTagBox(string tag, PointF center, Graphics g)
        {
            SizeF textSize = g.MeasureString(tag, tagFont);
            PointF upperLeft = new PointF(center.X - textSize.Width / 2, 
                                          center.Y - textSize.Height / 2);

            RectangleF tagRect = new RectangleF(upperLeft, textSize);
            tagRect.Inflate(3.0f, 3.0f);

            PointF[] tagPts = new PointF[5];
            tagPts[0] = tagRect.Location;
            tagPts[1] = new PointF(tagRect.Right, tagRect.Top);
            tagPts[2] = new PointF(tagRect.Right, tagRect.Bottom);
            tagPts[3] = new PointF(tagRect.Left, tagRect.Bottom);
            tagPts[4] = tagRect.Location;

            GraphicsPath path = new GraphicsPath();
            Corners c = new Corners(tagPts, 4.0);
            c.Execute(path);

            try
            {
                g.FillPath(tagBrush, path); 
                g.DrawString(tag, tagFont, Brushes.Black, upperLeft);
            }
            catch
            {
                /* some problem drawing */
            }
        }

        public static void DrawShape(PointF[] pts, Graphics g, Pen linePen)
        {
            GraphicsPath path = new GraphicsPath();
            Corners c = new Corners(pts, 2.0);
            //c.Execute(path);

            try
            {
                g.DrawPolygon(linePen, pts);
                //g.DrawPath(linePen, path); //TODO: fix
            }
            catch
            {
                /* Very screwed up polygon */
            }
        }



        public static void ProjectBox(RectangleF modelRect, GeneralMatrix H, Graphics g, Pen linePen)
        {
            PointF[] points = new PointF[5];
            GeneralMatrix projected, corner;

            // Upper Left
            corner = MatrixFromPoint2D(new Point2D(modelRect.Left, modelRect.Top));
            projected = H * corner;
            HomogeneousNormalize(projected);
            points[0].X = (float)projected.Array[0][0];
            points[0].Y = (float)projected.Array[1][0];

            // Upper Right
            corner = MatrixFromPoint2D(new Point2D(modelRect.Right, modelRect.Top));
            projected = H * corner;
            HomogeneousNormalize(projected);
            points[1].X = (float)projected.Array[0][0];
            points[1].Y = (float)projected.Array[1][0];

            // Lower Right
            corner = MatrixFromPoint2D(new Point2D(modelRect.Right, modelRect.Bottom));
            projected = H * corner;
            HomogeneousNormalize(projected);
            points[2].X = (float)projected.Array[0][0];
            points[2].Y = (float)projected.Array[1][0];

            // Lower Left
            corner = MatrixFromPoint2D(new Point2D(modelRect.Left, modelRect.Bottom));
            projected = H * corner;
            HomogeneousNormalize(projected);
            points[3].X = (float)projected.Array[0][0];
            points[3].Y = (float)projected.Array[1][0];

            // close the box
            points[4].X = points[0].X;
            points[4].Y = points[0].Y;

            GraphicsPath path = new GraphicsPath();
            Corners c = new Corners(points, 2.0);
            c.Execute(path);

            try
            {
                g.DrawPath(linePen, path);
                //g.DrawPolygon(linePen, points);
            }
            catch
            {
                /* Very screwed up polygon */
            }

        }

        
       
        public static void ProjectBox(Size modelSize, GeneralMatrix H, Graphics g, Pen linePen)
        {
            ProjectBox(new RectangleF(0F, 0F, (float)modelSize.Width, (float)modelSize.Height), H, g, linePen);
        }


        internal static void DrawMatches(IEnumerable<KeypointMatch> matches, SurfKeypoint[] model, SurfKeypoint[] data, Graphics g, float dataOffsetX)
        {
            foreach (KeypointMatch match in matches)
            {
                SurfKeypoint modelPt = model[match.modelIdx];
                SurfKeypoint dataPt = data[match.dataIdx];

                PointF startPt = new PointF((float)modelPt.x, (float)modelPt.y);
                PointF endPt = new PointF((float)dataPt.x + dataOffsetX, (float)dataPt.y);
                g.DrawLine(Pens.Yellow, startPt, endPt);
            }

        }

        public static void DrawKeypoints(IEnumerable<int> indices, IList<SurfKeypoint> keypoints, Graphics g)
        {
            PrepareGraphics(g);

            foreach (int idx in indices)
            {
                SurfKeypoint keyPoint = keypoints[idx];

                float diam = (float)keyPoint.strength / 5.0F;

                g.FillEllipse(Brushes.White,
                    (float)keyPoint.x - diam / 2, (float)keyPoint.y - diam / 2, diam, diam);

                PointF startPt = new PointF((float)keyPoint.x, (float)keyPoint.y);
                PointF endPt = vectorHead(startPt, -1 * keyPoint.orientation, keyPoint.scale * 2.5);

                g.DrawLine(Pens.Yellow, startPt, endPt);

                //  Console.WriteLine("({0},{1}) scale:{2} ori:{3} strength:{4}",
                //      keyPoint.x, keyPoint.y, keyPoint.scale, keyPoint.orientation, keyPoint.strength);
            }            
        }

        public static void DrawKeypoints(ICollection<SurfKeypoint> keypoints, Graphics g)
        {
            // Without the indices argument, just draw all of the keypoints
            int[] allIndices = new int[keypoints.Count];
            for (int i = 0; i < keypoints.Count; i++)
                allIndices[i] = i;

            DrawKeypoints((IEnumerable<int>)allIndices, (IList<SurfKeypoint>)keypoints, g);
        }

        private static PointF vectorHead(PointF vectorTail, double angle, double size)
        {
            return new PointF(vectorTail.X + (float)(Math.Cos(angle) * size),
                              vectorTail.Y + (float)(Math.Sin(angle) * size));
        }

    }

    #region Rounded Object Visualization
    abstract class CornersItem
    {
        private bool visible;
        public CornersItem() 
        {
            visible = true;
        }
        public abstract void addToPath(GraphicsPath path);
        public bool Visible
        {
            get { return visible;}
            set { visible = value; }
        }
    }
    class CornersLine : CornersItem
    {
        private double x1;
        private double y1;
        private double x2;
        private double y2;
        private double newX1;
        private double newY1;
        private double newX2;
        private double newY2;

        public double X1
        {
            get { return x1; }
            set { x1 = value; newX1 = value;}
        }
        public double Y1
        {
            get { return y1; }
            set { y1 = value; newY1 = value; }
        }
        public double X2
        {
            get { return x2; }
            set { x2 = value; newX2 = value; }
        }
        public double Y2
        {
            get { return y2; }
            set { y2 = value; newY2 = value; }
        }
        public double NewX1
        {
            get { return newX1; }
            set { newX1 = value; }
        }
        public double NewY1
        {
            get { return newY1; }
            set { newY1 = value; }
        }
        public double NewX2
        {
            get { return newX2; }
            set { newX2 = value; }
        }
        public double NewY2
        {
            get { return newY2; }
            set { newY2 = value; }
        }
        public override void addToPath(GraphicsPath path)
        {
            if (Visible) {
                if (Math.Sqrt(Math.Pow(newX1 - newX2, 2) + Math.Pow(newY1 - newY2, 2)) > 0.01)
                {
                    path.AddLine((float) newX1, (float)newY1, (float)newX2, (float)newY2);
                }
            }
        }

    }
    class CornersArc : CornersItem
    {
        private double x;
        private double y;
        private double width;
        private double height;
        private double startAngle;
        private double sweepAngle;

        public double X
        {
            get { return x; }
            set { x = value; }
        }

        public double Y
        {
            get { return y; }
            set { y = value; }
        }

        public double Width
        {
            get { return width; }
            set { width = value; }
        }
        public double Height
        {
            get { return height; }
            set { height = value; }
        }

        public double StartAngle
        {
            get { return startAngle; }
            set { startAngle = value; }
        }

        public double SweepAngle
        {
            get { return sweepAngle; }
            set { sweepAngle = value; }
        }

        public override void addToPath(GraphicsPath path)
        {
            if (Visible)
            {
               // path.AddArc((float)x, (float)y, (float)width, (float)height, (float)startAngle, (float)(sweepAngle));
            }
        }
    }
    public class Corners
    {
        private PointF[] points;
        private double radius;
        private List<CornersItem> list 
            = new List<CornersItem>();
        private void fillList()
        {
            CornersLine line;
            switch (points.Length)
            {
                case 2:
                    line = new CornersLine();
                    list.Add(line);
                    line.X1 = points[0].X;
                    line.Y1 = points[0].Y;
                    line.X2 = points[1].X;
                    line.Y2 = points[1].Y;
                    break;
                default:
                    PointF firstPoint = new PointF(points[0].X,points[0].Y);
                    for (int i = 1, j = 0; i < points.Length; i++)
                    {
                        line = new CornersLine();
                        list.Add(line);
                        line.X1 = points[j].X;
                        line.Y1 = points[j].Y;
                        ++j;
                        line.X2 = points[j].X;
                        line.Y2 = points[j].Y;
                        if (i != points.Length - 1)
                        {
                            CornersArc arc = new CornersArc();
                            list.Add(arc);
                        }
                        else
                        {
                            if ((firstPoint.X == points[j].X) && (firstPoint.Y == points[j].Y)) 
                            {
                                CornersArc arc = new CornersArc();
                                list.Add(arc);
                            }

                        }
                    }
                    break;
            }
        }

        public Corners(PointF[] points, double radius)
        {
            this.points = points;
            this.radius = radius;
            fillList();
        }

        public void Execute(GraphicsPath path) 
        {
            CornersLine line1,line2;
            CornersArc arc;

            int i = 0;
            while (true)
            {
                if (i == list.Count) break;
                line1 = list[i] as CornersLine;
                i++;
                if (i == list.Count) break;
                arc = list[i] as CornersArc;
                i++;
                if (i == list.Count)
                    line2 = list[0] as CornersLine;
                else
                    line2 = list[i] as CornersLine;
                CalculateRoundLines(line1, line2, arc);
            }

            for (int j = 0; j < list.Count; j++) 
            {
                (list[j] as CornersItem).addToPath(path);
            }
        }

        private void CalculateRoundLines(CornersLine line1, CornersLine line2, 
            CornersArc arc)
        {
            double f1 = Math.Atan2(line1.Y1 - line1.Y2, line1.X1 - line1.X2);
            double f2 = Math.Atan2(line2.Y2 - line2.Y1, line2.X2 - line2.X1);
            double alfa = f2 - f1;
            if ((alfa == 0) || (Math.Abs(alfa) == Math.PI)) 
                addWithoutArc(arc);
            else
                addWithArc(line1, line2, arc, f1, f2, alfa);
        }

        private static void addWithoutArc(CornersArc arc)
        {
            arc.Visible = false;
        }

        private void addWithArc(CornersLine line1, CornersLine line2, CornersArc arc, double f1, double f2, double alfa)
        {
            double s = radius / Math.Tan(alfa / 2);
            double line1Length = Math.Sqrt(Math.Pow(line1.X1 - line1.X2, 2) + Math.Pow(line1.Y1 - line1.Y2, 2));
            double line2Length = Math.Sqrt(Math.Pow(line2.X1 - line2.X2, 2) + Math.Pow(line2.Y1 - line2.Y2, 2));
            double newRadius = radius;

            if ((Math.Abs(s) > line1Length / 2) || (Math.Abs(s) > line2Length / 2))
            {
                if (s < 0)
                    s = -Math.Min(line1Length / 2, line2Length / 2);
                else
                    s = Math.Min(line1Length / 2, line2Length / 2);
                newRadius = s * Math.Tan(alfa / 2);
            }

            line1.NewX2 = line1.X2 + Math.Abs(s) * Math.Cos(f1);
            line1.NewY2 = line1.Y2 + Math.Abs(s) * Math.Sin(f1);
            line2.NewX1 = line2.X1 + Math.Abs(s) * Math.Cos(f2);
            line2.NewY1 = line2.Y1 + Math.Abs(s) * Math.Sin(f2);

            double circleCenterAngle = f1 + alfa / 2;
            double cs = newRadius / Math.Sin(alfa / 2);
            PointF circleCenter = new PointF();
            if (s > 0)
            {
                circleCenter.X = (float)(line1.X2 + cs * Math.Cos(circleCenterAngle));
                circleCenter.Y = (float)(line1.Y2 + cs * Math.Sin(circleCenterAngle));
            }
            else
            {
                circleCenter.X = (float)(line1.X2 - cs * Math.Cos(circleCenterAngle));
                circleCenter.Y = (float)(line1.Y2 - cs * Math.Sin(circleCenterAngle));
            }

            double firstAngle = Math.Atan2(line1.NewY2 - circleCenter.Y, line1.NewX2 - circleCenter.X);
            double secondAngle = Math.Atan2(line2.NewY1 - circleCenter.Y, line2.NewX1 - circleCenter.X);
            double startAngle = firstAngle;
            double sweepAngle = secondAngle - firstAngle;
            if (sweepAngle > Math.PI)
                sweepAngle = -(2 * Math.PI - sweepAngle);
            else
            {
                if (sweepAngle < -Math.PI)
                    sweepAngle = (2 * Math.PI + sweepAngle);
            }
            arc.X = circleCenter.X - newRadius;
            arc.Y = circleCenter.Y - newRadius;
            arc.Width = newRadius * 2;
            arc.Height = newRadius * 2;
            arc.StartAngle = startAngle * (180 / Math.PI);
            arc.SweepAngle = sweepAngle * (180 / Math.PI);
        }


    }
    #endregion

}
